/* Function to grab and convert Data from katanas .. */ 

#include "katana_utils.h"
#define MotorMax 6
#define MEMTHRESH 1000


CLMBase * initKatanaG()
{
  CLMBase* katanaG;
  CCdlSocket* device_G;
  CCplSerialCRC*  protocol_G;
  try {

    //Init device
    device_G = new CCdlSocket(IPADDR_G,PORTNB_G);
    // std::cout << "Success: port open" << std::endl;
    //Init protocol
    protocol_G = new CCplSerialCRC();
    protocol_G->init(device_G);
    // std::cout<< "Success: protocol initiated" << std::endl;
    //Init robot:
    katanaG = new CLMBase();
    katanaG->create(CFGFILE_G,protocol_G);
   
  } catch(Exception &e) {
    std::cout << "ERROR: " << e.message() << std::endl;
    exit(0);
  }
  std::cout << "Success: katana G initiated" << std::endl;
  return katanaG;
}

CLMBase *  initKatanaT()
{
  CLMBase* katanaT;
  CCdlSocket* device_T;
  CCplSerialCRC*  protocol_T;
  try {

    //Init device
    device_T = new CCdlSocket(IPADDR_T,PORTNB_T);
    // std::cout << "Success: port open" << std::endl;
    //Init protocol
    protocol_T = new CCplSerialCRC();
    protocol_T->init(device_T);
    // std::cout<< "Success: protocol initiated" << std::endl;
    //Init robot:
    katanaT = new CLMBase();
    katanaT->create(CFGFILE_T,protocol_T);
   
  } catch(Exception &e) {
    std::cout << "ERROR: " << e.message() << std::endl;
    exit(0);
  }
  std::cout << "Success: katana T initiated" << std::endl;
  return katanaT;
}

void getEncodersValues(CLMBase * katana,int * vals,struct timeval begin)
{
  // stores each encoder in the * vals table 
  struct timeval now;
  struct timezone tz;
  const TKatMOT* mot;
  mot = katana->GetBase()->GetMOT();
  gettimeofday(&now,&tz);
  int elapsed_sec;
  long elapsed;
  elapsed = now.tv_usec  - begin.tv_usec;
  elapsed_sec = now.tv_sec - begin.tv_sec;
  elapsed = elapsed_sec * 1000000 + elapsed;
  vals[0] = elapsed/1000;

  katana->GetBase()->recvMPS();    

  for (int i=0; i<MotorMax; i++) {
      //Get encoder values
    vals[i+1] = mot->arr[i].GetPVP()->pos;

  } 
}


void getAnglesValues(CLMBase * katana,Vector& values)
{
  // very fucking slow function call ... due to getMotorEncoders() 
  values.Resize(MotorMax);
  Vector encoders(MotorMax);
  const TKatMOT* mot;
  mot = katana->GetBase()->GetMOT();
  katana->GetBase()->recvMPS();    
  for (int i=0; i<MotorMax; i++) {
    //Get encoder values
    encoders(i) = mot->arr[i].GetPVP()->pos;
    
  } 
  encodersToAngle(katana,encoders,values);
}

void encodersToAngle(CLMBase * katana,Vector& encoders, Vector& angles)
{
  const TKatMOT* mot;
  KNI::KinematicParameters joint;
  mot = katana->GetBase()->GetMOT(); 
  angles.Resize(MotorMax);
  for(int m=0;m<MotorMax;m++)
    {
      joint.epc         = mot->arr[m].GetInitialParameters()->encodersPerCycle;
      joint.encOffset   = mot->arr[m].GetInitialParameters()->encoderOffset;
      joint.angleOffset = mot->arr[m].GetInitialParameters()->angleOffset;
      joint.angleStop   = mot->arr[m].GetInitialParameters()->angleStop;
      joint.rotDir      = mot->arr[m].GetInitialParameters()->rotationDirection;
      angles(m)=KNI_MHF::enc2rad((int)encoders(m),joint.angleOffset, joint.epc, 
				 joint.encOffset, joint.rotDir);
    }

}

Matrix encodersToMatrix(CLMBase * katana,int ** vals,int nValues)
{
  Matrix res(nValues,MotorMax+1);
  const TKatMOT* mot;
  KNI::KinematicParameters joint;
  mot = katana->GetBase()->GetMOT(); 
  for(int i=0;i<nValues;i++)
	{
	  res(i,0)=vals[i][0]; // ms time
	}
  for(int m=0;m<MotorMax;m++)
    {
      joint.epc         = mot->arr[m].GetInitialParameters()->encodersPerCycle;
      joint.encOffset   = mot->arr[m].GetInitialParameters()->encoderOffset;
      joint.angleOffset = mot->arr[m].GetInitialParameters()->angleOffset;
      joint.angleStop   = mot->arr[m].GetInitialParameters()->angleStop;
      joint.rotDir      = mot->arr[m].GetInitialParameters()->rotationDirection;
      for(int i=0;i<nValues;i++)
	{
	  res(i,m+1)=KNI_MHF::enc2rad(vals[i][m+1],joint.angleOffset, joint.epc, 
				      joint.encOffset, joint.rotDir);
	}
    }
  return res;
}

Matrix demonstration(CLMBase * katana) // grab a demo from katana and store to a matrix object
{
  char c=0;

  int nbStep=0;
  int ** values = new int*[MEMTHRESH]; // 40 s of demonstration 
  Matrix result;
  struct timeval begin;
  struct timezone tz;
  std::cout << "press space to start demo" <<std::endl;
  while(c!=' ')
    c=getch();
  c='0';
  gettimeofday(&begin,&tz);

  std::cout << "Move the robot. Press SPACE to stop." << std::endl;
  while((c!=' ') && (nbStep < MEMTHRESH)){
    c=getch();
    //Get data in joint space
    values[nbStep]=new int[7];
    getEncodersValues(katana,values[nbStep],begin);
    nbStep++;
  }
  result = encodersToMatrix(katana,values,nbStep);
  delete [] values;
  return result; 
}

void moveToAnglesValues(CLMBase * katana,Vector& values)
{
  const TKatMOT* mot;
  KNI::KinematicParameters joint;
  mot = katana->GetBase()->GetMOT(); 
  int * enc = new int[MotorMax];
  int encVal;
  for (int i=0;i<MotorMax;i++) // Converting radian to encoders values 
    {
      joint.epc         = mot->arr[i].GetInitialParameters()->encodersPerCycle;
      joint.encOffset   = mot->arr[i].GetInitialParameters()->encoderOffset;
      joint.angleOffset = mot->arr[i].GetInitialParameters()->angleOffset;
      joint.angleStop   = mot->arr[i].GetInitialParameters()->angleStop;
      joint.rotDir      = mot->arr[i].GetInitialParameters()->rotationDirection;

      encVal = KNI_MHF::rad2enc((double) values(i), 
				       joint.angleOffset, 
				       joint.epc, 
				       joint.encOffset, 
				       joint.rotDir);
      if(!mot->arr[i].checkEncoderInRange(encVal))  // prevents forbidden values 
	   {
	     encVal =  mot->arr[i].GetInitialParameters()->encoderOffset; 
	   }
      enc[i] = encVal;
    }
  for(int i =0;i<6;i++)
    {  
      katana->mov(i,enc[i]);			   
    }
}


/*controle du brase vitesse par moteur*/
void controle_moteur_vitesse(CLMBase *katana,int idx, int velocity)
{
  int				i = 0;
  char				m = '\0';
  const TKatMOT			*motor;
  bool change = true;
  /*parametre pour le calcul de temps*/
  long Secondes;
  long MicroSecondes;
  struct timeval Input;
  struct timeval Output;
  int min,max;

  /*on met la vitesse de ce moteur a velocity*/
  /*on ne peut pas mettre des vitesses negatives*/
  /*lorsqu'on rentre un nombre negatif la vitesse du moteur est tres rapide*/
  /*position d arriver*/
  /*on reste entre la position min et max*/

  min = katana->GetBase()->GetMOT()->arr[idx].GetEncoderMinPos() + 3000;
  max = katana->GetBase()->GetMOT()->arr[idx].GetEncoderMaxPos() - 3000;
  if(velocity >= 0)
    {
      if (idx == 0) {if (velocity > 100) velocity = 100;}
      
      if(velocity == 0)
	{
	  katana->freezeMotor(idx);
	}
      else
	{
	  katana->setMotorVelocityLimit(idx, velocity);
	  katana->moveMotorToEnc(idx, max, false);
	}
    }
  else if (velocity < 0)
    {  
      if (idx == 0) {if (velocity > 100) velocity = 100;}
      if(velocity == 0)
	{
	  katana->freezeMotor(idx);
	}
      else
	{
	  katana->setMotorVelocityLimit(idx, -velocity);
	  katana->moveMotorToEnc(idx, min, false);
	}
    } 
}


void controle_moteur_vitesse_rads(CLMBase *katana,
				  int m, /* motor index */
				  float velocity) /* velocity in rad/s */
{
  const TKatMOT* mot = katana->GetBase()->GetMOT(); 
  int epc         = mot->arr[m].GetInitialParameters()->encodersPerCycle;
  int rotDir      = mot->arr[m].GetInitialParameters()->rotationDirection;
  int enc_speed = round(  velocity * epc * rotDir / (2*M_PI) );
  controle_moteur_vitesse(katana,m,enc_speed);
}
